﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Xml;
using System.Xml.Serialization;
using System.IO;
using System.IO.Ports;
using System.Diagnostics;
using Phidgets;
using Phidgets.Events;
using System.Net;
using System.Net.Sockets;
using System.Threading;

namespace RCU_Final
{
    public partial class RCU : Form
    {
        enum Dumping { All, NoDump, NoReturn }

        #region constants
        const int BIN_INDEX = 1;
        const int LISTEN_PORT = 11000;
        const int SEND_PORT = 11001;
        const int LISTEN_TIMEOUT = 500;
        const int SERIAL_WRITE_TIMEOUT = 100;
        const int PHIDGET_WAIT_TIMEOUT = 3000;
        const string OCU_ADDRESS = "192.168.1.153";
        #endregion

        #region variables

        /*serial port*/
        private SerialPort robo1, robo2;
        
        /*threads*/
        private Thread UDP_Monitor_Thread, Serial_Monitor_Thread, Thread_Monitor_Thread;

        /*thread booleans*/
        private volatile bool UDPShouldStop = false, SerialShouldStop = false, ThreadMonitorShouldStop = false; 

        /*Phidget devices*/
        private InterfaceKit ifkit;
        private Servo phidgetMotors;

        /*dump bucket permissions*/
        private static Dumping permission = Dumping.All;

        /*locks*/
        private static readonly object dumpLocker = new object();

        #endregion

        #region constructor & startup
        public RCU()
        {
            InitializeComponent();
        }

        private void RCU_Load(object sender, EventArgs e)
        {
            try
            {
                /*add exit handler*/
                System.Windows.Forms.Application.ApplicationExit += new EventHandler(Application_ApplicationExit);
                /*open serial ports*/
                robo1 = new SerialPort("COM1", 115200, Parity.None, 8, StopBits.One);
                robo1.WriteTimeout = SERIAL_WRITE_TIMEOUT;
                robo2 = new SerialPort("COM2", 115200, Parity.None, 8, StopBits.One);
                robo2.WriteTimeout = SERIAL_WRITE_TIMEOUT;
                robo1.Open();
                robo2.Open();

                /*open phidget devices*/
                phidgetMotors = new Servo();
                phidgetMotors.open();
                phidgetMotors.waitForAttachment(PHIDGET_WAIT_TIMEOUT);

                for (int i = 0; i < phidgetMotors.servos.Count; i++)
                {
                    phidgetMotors.servos[i].Engaged = false;
                    phidgetMotors.servos[i].Type = ServoServo.ServoType.RAW_us_MODE;
                    phidgetMotors.servos[i].Position = 1500;
                    phidgetMotors.servos[i].Engaged = true;
                }
                /*open phidgets 8/8/8*/
                ifkit = new InterfaceKit();
                /*add watcher for the digital inputs*/
                ifkit.InputChange += new InputChangeEventHandler(iKit_InputChange);
                ifkit.open();
                ifkit.waitForAttachment(PHIDGET_WAIT_TIMEOUT);

                /*launch threads*/
                UDP_Monitor_Thread = new Thread(new ThreadStart(UDP_Monitor));
                UDP_Monitor_Thread.Start();
                Serial_Monitor_Thread = new Thread(new ThreadStart(Serial_Monitor));
                Serial_Monitor_Thread.Start();
                Thread_Monitor_Thread = new Thread(new ThreadStart(Thread_Monitor));
                Thread_Monitor_Thread.Start();
            }
            catch (Exception ex)
            {
                Application.Exit();
            }
        }
       #endregion

        #region exit method
        void Application_ApplicationExit(object sender, EventArgs e)
        {
            /*on exit, close threads*/
            Thread_Monitor_Stop();
            while (Thread_Monitor_Thread != null && Thread_Monitor_Thread.IsAlive) /*wait for thread to die*/
                Thread.Sleep(100);
            UDP_Stop();
            Serial_Stop();
            while ((UDP_Monitor_Thread != null && UDP_Monitor_Thread.IsAlive) || (Serial_Monitor_Thread != null && Serial_Monitor_Thread.IsAlive)) /*wait for threads to die*/
                Thread.Sleep(100);
            /*close serial ports*/
            if (robo1 != null)
                robo1.Close();
            if (robo2 != null)
                robo2.Close();
            /*close phidget devices*/
            if (ifkit != null)
                ifkit.close();
            if (phidgetMotors != null)
                phidgetMotors.close();
        }
        #endregion

        #region phidgets 8/8/8 events
        void iKit_InputChange(object sender, InputChangeEventArgs e)
        {
            try
            {
                switch (e.Index)
                {
                    case 0: /*cancel/enable dump*/
                        lock (dumpLocker)
                        {
                            if (e.Value == false)
                                permission = Dumping.All;
                            else
                                permission = Dumping.NoReturn;
                            phidgetMotors.servos[BIN_INDEX].Position = 1500;
                        }
                        break;
                    case 1: /*cancel/enable return*/
                        lock (dumpLocker)
                        {
                            if (e.Value == false)
                                permission = Dumping.All;
                            else
                                permission = Dumping.NoDump;
                            phidgetMotors.servos[BIN_INDEX].Position = 1500;
                        }
                        break;
                }
            }
            catch (Exception ex)
            {
            }
        }
        #endregion

        #region Thread Methods

        #region UDP Monitor Code
        public void UDP_Monitor()
        {            
            /*set connection parameters*/
            IPEndPoint endPoint = new IPEndPoint(IPAddress.Any, LISTEN_PORT);
            UdpClient listener = new UdpClient(endPoint);
            listener.Client.ReceiveTimeout = LISTEN_TIMEOUT;
            byte[] buffer;
            MemoryStream bufferStream;
            XmlSerializer deserializer = new XmlSerializer(typeof(LunabotOutputState));
            
            while (!UDPShouldStop)
            {
                try
                {
                    /*attempt to receive data*/
                    buffer = listener.Receive(ref endPoint);
                    /*deserialize*/
                    bufferStream = new MemoryStream(buffer);
                    LunabotOutputState state = (LunabotOutputState)deserializer.Deserialize(bufferStream);
                    updateSystem(state);
                }
                catch (SocketException ex)
                {
                    try
                    {
                        /*haven't received an update in time...kill all actions*/
                        LunabotOutputState nullState = new LunabotOutputState();
                        updateSystem(nullState);
                    }
                    catch (Exception ex2)
                    {
                    }
                }
                catch (Exception ex)
                {
                }
            }
        }

        public void updateSystem(LunabotOutputState state)
        {
            try
            {
                if (state == null)
                    return;
                /*handle collection bin*/
                lock (dumpLocker)
                {
                    switch (permission)
                    {
                        case Dumping.All:
                            phidgetMotors.servos[1].Position = state.collectionBin = (state.collectionBin > 2000) ? 2000 : ((state.collectionBin < 1000) ? 1000 : state.collectionBin);
                            break;
                        case Dumping.NoReturn:
                            phidgetMotors.servos[1].Position = state.collectionBin = (state.collectionBin > 1500) ? 1500 : ((state.collectionBin < 1000) ? 1000 : state.collectionBin);
                            break;
                        case Dumping.NoDump:
                            phidgetMotors.servos[1].Position = state.collectionBin = (state.collectionBin > 2000) ? 2000 : ((state.collectionBin < 1500) ? 1500 : state.collectionBin);
                            break;
                    }
                }

                /*handle digging mechanism*/
                phidgetMotors.servos[0].Position = state.diggingMechanism = (state.diggingMechanism > 2000) ? 2000 : ((state.diggingMechanism < 1000) ? 1000 : state.diggingMechanism);

                /*handle digging head*/
                switch (state.diggingHead)
                {
                    case LunabotOutputState.ActionState.Down:
                        ifkit.outputs[2] = false;
                        ifkit.outputs[3] = true;
                        break;
                    case LunabotOutputState.ActionState.Up:
                        ifkit.outputs[3] = false;
                        ifkit.outputs[2] = true;
                        break;
                    case LunabotOutputState.ActionState.Off:
                        ifkit.outputs[2] = false;
                        ifkit.outputs[3] = false;
                        break;
                }

                /*process motor values*/
                int motorLeft = state.motorSpeedLeft =  (state.motorSpeedLeft < -1000) ? -1000 : ((state.motorSpeedLeft > 1000) ? 1000 : state.motorSpeedLeft);
                int motorRight = state.motorSpeedRight =  ((state.motorSpeedRight < -1000) ? -1000 : ((state.motorSpeedRight > 1000) ? 1000 : state.motorSpeedRight)) * -1;
                string roboCommand = "!M " + motorLeft.ToString() + " " + motorRight.ToString() + "_?V" + "\r\n";
                robo1.Write(roboCommand);
                robo2.Write(roboCommand);

                /*update property grid*/
                BeginInvoke((MethodInvoker)delegate {  outputStateGrid.SelectedObject = state; });
            }
            catch (Exception ex)
            {
            }
        }

        public void UDP_Stop()
        {
            UDPShouldStop = true;
        }
        #endregion

        #region Serial Monitor Code
        public void Serial_Monitor()
        {
            LunabotHealthState state;
            string buffer;
            MemoryStream stream;
            XmlSerializer serializer = new XmlSerializer(typeof(LunabotHealthState));
            byte[] output;
            UdpClient healthServer = new UdpClient(OCU_ADDRESS, SEND_PORT);
            while (!SerialShouldStop)
            {
                try
                {
                    /*read from robo1 (both robos should return same result, only read from one for speed purposes)*/
                    buffer = robo1.ReadTo("\r");
                    if (buffer.Length == 0 || buffer[0] != 'V')
                        continue;
                    else
                    {
                        state = new LunabotHealthState();
                        /*parse string to get voltages*/
                        string[] voltages = (buffer.Replace("V=", "")).Split(':');
                        if (voltages.Length != 3)
                            continue;
                        state.electronicsBatteryVoltage = (float)(Int32.Parse(voltages[0])/10.0);
                        state.driveBatteryVoltage = (float)(Int32.Parse(voltages[1])/ 10.0);

                        /*grab sensor values - phidgets, range finder*/
                        state.excavationBatteryVoltage = (float)(ifkit.sensors[0].Value/1000.0*14.4);
                        state.canDump = ifkit.inputs[2];

                        /*grab system state - the actual LunabotOutputState in use*/
                        state.OutputState = (LunabotOutputState)outputStateGrid.SelectedObject;

                        /*update property grid*/
                        BeginInvoke((MethodInvoker)delegate { healthStateGrid.SelectedObject = state; }); 

                        /*send back to OCU*/
                        stream = new MemoryStream();
                        serializer.Serialize(stream, state);
                        output = stream.ToArray();
                        healthServer.Send(output, output.Length);
                    }

                }
                catch (Exception ex)
                {
                }
            }
        }
        public void Serial_Stop()
        {
            SerialShouldStop = true;
        }
        #endregion

        #region Thread Monitor Code
        public void Thread_Monitor()
        {
            while (!ThreadMonitorShouldStop)
            {
                /*check if each thread is still running*/
                if (UDP_Monitor_Thread == null || UDP_Monitor_Thread.IsAlive != true)
                {
                    UDPShouldStop = false;
                    UDP_Monitor_Thread = new Thread(new ThreadStart(UDP_Monitor));
                    UDP_Monitor_Thread.Start();
                }
                if (Serial_Monitor_Thread == null || Serial_Monitor_Thread.IsAlive != true)
                {
                    SerialShouldStop = false;
                    Serial_Monitor_Thread = new Thread(new ThreadStart(Serial_Monitor));
                    Serial_Monitor_Thread.Start();
                }
                /*sleep a while to lower CPU usage*/
                Thread.Sleep(1000);
                
            }
        }
        
        public void Thread_Monitor_Stop()
        {
            ThreadMonitorShouldStop = true;
        }
        #endregion

        #endregion
    }
}
